#include "WPILib.h"


const int startHeight = 0;
const int height1 = 1;
const int height2 = 2;
const int height3 = 3;
const int armPower = 1.0;

class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot;
	RobotDrive arm;
	Joystick leftStick; 
	Joystick rightStick;
	Gyro gyro;
	Encoder AnEncoder;
	
	
public:
	RobotDemo(void):
			myRobot(1, 2, 3, 4),	// these must be initialized in the same order
			arm(5, 6),
			leftStick(1),		// as they are declared above.
			rightStick(2),
			gyro(1),
			AnEncoder(2, 3)
		{
			myRobot.SetExpiration(0.1);
		}
	void Autonomous(void);
	void OperatorControl(void);
	void GyroTurn(int degrees){
		gyro.Reset();
			while( gyro.GetAngle() <= degrees-10 )
			{
				myRobot.Drive( 0.5, 1.0 );
			}
			while( gyro.GetAngle() <= degrees-5 )
			{
				myRobot.Drive(0.2, 1.0);
			}
			while( gyro.GetAngle() <= degrees )
			{
				myRobot.Drive(0.1, 1.0);	
			}
			myRobot.Drive(0.4, -1.0); //Brake
			Wait(0.1);
			myRobot.Drive( 0.0, 0.0 );
	}
	
	void FULLFUCKINGPOWER(int degrees)
	{
		gyro.Reset();
		while( gyro.GetAngle() <= degrees )
		{
			myRobot.Drive(1.0, 1.0 );
		}
		myRobot.Drive( 0.0, 0.0 );
	}	
	
	void SetArmHeight() //Will have to move to main control loop later
	{
		bool joyBottom;
		bool joy1;
		bool joy2;
		bool joy3;
		
		if(leftStick.GetRawButton(2)) //Set True/False when joystick btn is pressed
		{
			joyBottom = true;
			joy1 = false;
			joy2 = false;
			joy3 = false;
		}
		
		else if(leftStick.GetRawButton(4))
		{
			joyBottom = false;
			joy1 = true;
			joy2 = false;
			joy3 = false;
		}
		
		else if(leftStick.GetRawButton(3))
		{
			joyBottom = false;
			joy1 = false;
			joy2 = true;
			joy3 = false;
		}
		
		else if(leftStick.GetRawButton(5))
		{
			joyBottom = false;
			joy1 = false;
			joy2 = false;
			joy3 = true;
		}
		
		//===================================================//
		
		if(joyBottom && (AnEncoder.Get > startHeight)) //Move arm based on joystick T/F
		{
			arm.Drive(-armPower, 0.0);
			joyBottom = false;
		}
		
		else if(joy1 && (AnEncoder.Get < height1))
		{
			arm.Drive(armPower, 0.0);
			joy1 = false;
		}
		
		else if(joy1 && (AnEncoder.Get > height1))
		{
			arm.Drive(-armPower, 0.0);
			joy1 = false;
		}
		
		else if(joy2 && (AnEncoder.Get < height2))
		{
			arm.Drive(armPower, 0.0);
			joy2 = false;
		}
		
		else if(joy2 && (AnEncoder.Get > height2))
		{
			arm.Drive(-armPower, 0.0);
			joy2 = false;
		}		
		
		else if(joy3 && (AnEncoder.Get < height3))
		{
			arm.Drive(armPower, 0.0);
			joy3 = false;
		}
		
		else if(joy3 && (AnEncoder.Get > height3))
		{
			arm.Drive(-armPower, 0.0);
			joy3 = false;
		}
				
	}


};
